home *** CD-ROM | disk | FTP | other *** search
/ Games of Daze / Infomagic - Games of Daze (Summer 1995) (Disc 1 of 2).iso / x2ftp / msdos / math / newmat08 / fft.cpp < prev    next >
C/C++ Source or Header  |  1995-01-11  |  7KB  |  215 lines
  1. //$$ fft.cpp                         Fast fourier transform
  2.  
  3. // Copyright (C) 1991,2,3,4: R B Davies
  4.  
  5.  
  6. #define WANT_MATH
  7. // #define WANT_STREAM
  8.  
  9. #include "include.h"
  10.  
  11. #include "newmatap.h"
  12.  
  13.  
  14. static void cossin(int n, int d, Real& c, Real& s)
  15. // calculate cos(twopi*n/d) and sin(twopi*n/d)
  16. // minimise roundoff error
  17. {
  18.    long n4 = n * 4; int sector = (int)floor( (Real)n4 / (Real)d + 0.5 );
  19.    n4 -= sector * d;
  20.    if (sector < 0) sector = 3 - (3 - sector) % 4; else sector %= 4;
  21.    Real ratio = 1.5707963267948966192 * (Real)n4 / (Real)d;
  22.  
  23.    switch (sector)
  24.    {
  25.    case 0: c =  cos(ratio); s =  sin(ratio); break;
  26.    case 1: c = -sin(ratio); s =  cos(ratio); break;
  27.    case 2: c = -cos(ratio); s = -sin(ratio); break;
  28.    case 3: c =  sin(ratio); s = -cos(ratio); break;
  29.    }
  30. }
  31.  
  32. static void fftstep(ColumnVector& A, ColumnVector& B, ColumnVector& X,
  33.    ColumnVector& Y, int after, int now, int before)
  34. {
  35.    Tracer trace("FFT(step)");
  36.    // const Real twopi = 6.2831853071795864769;
  37.    const int gamma = after * before;  const int delta = now * after;
  38.    // const Real angle = twopi / delta;  Real temp;
  39.    // Real r_omega = cos(angle);  Real i_omega = -sin(angle);
  40.    Real r_arg = 1.0;  Real i_arg = 0.0;
  41.    Real* x = X.Store();  Real* y = Y.Store();   // pointers to array storage
  42.    const int m = A.Nrows() - gamma;
  43.  
  44.    for (int j = 0; j < now; j++)
  45.    {
  46.       Real* a = A.Store(); Real* b = B.Store(); // pointers to array storage
  47.       Real* x1 = x; Real* y1 = y; x += after; y += after;
  48.       for (int ia = 0; ia < after; ia++)
  49.       {
  50.      // generate sins & cosines explicitly rather than iteratively
  51.      // for more accuracy; but slower
  52.      cossin(-(j*after+ia), delta, r_arg, i_arg);
  53.  
  54.      Real* a1 = a++; Real* b1 = b++; Real* x2 = x1++; Real* y2 = y1++;
  55.      if (now==2)
  56.      {
  57.         int ib = before; while (ib--)
  58.         {
  59.            Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
  60.            Real r_value = *a2; Real i_value = *b2;
  61.            *x2 = r_value * r_arg - i_value * i_arg + *(a2-gamma);
  62.            *y2 = r_value * i_arg + i_value * r_arg + *(b2-gamma);
  63.            x2 += delta; y2 += delta;
  64.         }
  65.      }
  66.      else
  67.      {
  68.         int ib = before; while (ib--)
  69.         {
  70.            Real* a2 = m + a1; Real* b2 = m + b1; a1 += after; b1 += after;
  71.            Real r_value = *a2; Real i_value = *b2;
  72.            int in = now-1; while (in--)
  73.            {
  74.           // it should be possible to make this faster
  75.           // hand code for now = 2,3,4,5,8
  76.           // use symmetry to halve number of operations
  77.           a2 -= gamma; b2 -= gamma;  Real temp = r_value;
  78.           r_value = r_value * r_arg - i_value * i_arg + *a2;
  79.           i_value = temp    * i_arg + i_value * r_arg + *b2;
  80.            }
  81.            *x2 = r_value; *y2 = i_value;   x2 += delta; y2 += delta;
  82.         }
  83.      }
  84.  
  85.          // temp = r_arg;
  86.          // r_arg = r_arg * r_omega - i_arg * i_omega;
  87.          // i_arg = temp  * i_omega + i_arg * r_omega;
  88.  
  89.       }
  90.    }
  91. }
  92.  
  93.  
  94. void FFT(const ColumnVector& U, const ColumnVector& V,
  95.    ColumnVector& X, ColumnVector& Y)
  96. {
  97.    // from Carl de Boor (1980), Siam J Sci Stat Comput, 1 173-8
  98.    Tracer trace("FFT");
  99.    const int n = U.Nrows();                     // length of arrays
  100.    if (n != V.Nrows() || n == 0)
  101.       Throw(ProgramException("Vector lengths unequal or zero", U, V));
  102.    ColumnVector B = V;
  103.    ColumnVector A = U;
  104.    X.ReDimension(n); Y.ReDimension(n);
  105.    const int nextmx = 8;
  106. #ifndef ATandT
  107.    int prime[8] = { 2,3,5,7,11,13,17,19 };
  108. #else
  109.    int prime[8];
  110.    prime[0]=2; prime[1]=3; prime[2]=5; prime[3]=7;
  111.    prime[4]=11; prime[5]=13; prime[6]=17; prime[7]=19;
  112. #endif
  113.    int after = 1; int before = n; int next = 0; Boolean inzee = TRUE;
  114.  
  115.    do
  116.    {
  117.       int now, b1;
  118.       for (;;)
  119.       {
  120.      if (next < nextmx) now = prime[next];
  121.      b1 = before / now;  if (b1 * now == before) break;
  122.      next++; now += 2;
  123.       }
  124.       before = b1;
  125.  
  126.       if (inzee) fftstep(A, B, X, Y, after, now, before);
  127.       else fftstep(X, Y, A, B, after, now, before);
  128.  
  129.       inzee = !inzee; after *= now;
  130.    }
  131.    while (before != 1);
  132.  
  133.    if (inzee) { A.Release(); X = A; B.Release(); Y = B; }
  134. }
  135.  
  136.  
  137. void FFTI(const ColumnVector& U, const ColumnVector& V,
  138.    ColumnVector& X, ColumnVector& Y)
  139. {
  140.    // Inverse transform
  141.    Tracer trace("FFTI");
  142.    FFT(U,-V,X,Y);
  143.    const Real n = X.Nrows(); X = X / n; Y = Y / (-n);
  144. }
  145.  
  146. void RealFFT(const ColumnVector& U, ColumnVector& X, ColumnVector& Y)
  147. {
  148.    // Fourier transform of a real series
  149.    Tracer trace("RealFFT");
  150.    const int n = U.Nrows();                     // length of arrays
  151.    const int n2 = n / 2;
  152.    if (n != 2 * n2)
  153.       Throw(ProgramException("Vector length not multiple of 2", U));
  154.    ColumnVector A(n2), B(n2);
  155.    Real* a = A.Store(); Real* b = B.Store(); Real* u = U.Store(); int i = n2;
  156.    while (i--) { *a++ = *u++; *b++ = *u++; }
  157.    FFT(A,B,A,B);
  158.    int n21 = n2 + 1;
  159.    X.ReDimension(n21); Y.ReDimension(n21);
  160.    i = n2 - 1;
  161.    a = A.Store(); b = B.Store();              // first els of A and B
  162.    Real* an = a + i; Real* bn = b + i;        // last els of A and B
  163.    Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
  164.    Real* xn = x + n2; Real* yn = y + n2;      // last els of X and Y
  165.  
  166.    *x++ = *a + *b; *y++ = 0.0;                // first complex element
  167.    *xn-- = *a++ - *b++; *yn-- = 0.0;          // last complex element
  168.  
  169.    int j = -1; i = n2/2;
  170.    while (i--)
  171.    {
  172.       Real c,s; cossin(j--,n,c,s);
  173.       Real am = *a - *an; Real ap = *a++ + *an--;
  174.       Real bm = *b - *bn; Real bp = *b++ + *bn--;
  175.       Real samcbp = s * am + c * bp; Real sbpcam = s * bp - c * am;
  176.       *x++  =  0.5 * ( ap + samcbp); *y++  =  0.5 * ( bm + sbpcam);
  177.       *xn-- =  0.5 * ( ap - samcbp); *yn-- =  0.5 * (-bm + sbpcam);
  178.    }
  179. }
  180.  
  181. void RealFFTI(const ColumnVector& A, const ColumnVector& B, ColumnVector& U)
  182. {
  183.    // inverse of a Fourier transform of a real series
  184.    Tracer trace("RealFFTI");
  185.    const int n21 = A.Nrows();                     // length of arrays
  186.    if (n21 != B.Nrows() || n21 == 0)
  187.       Throw(ProgramException("Vector lengths unequal or zero", A, B));
  188.    const int n2 = n21 - 1;  const int n = 2 * n2;  int i = n2 - 1;
  189.  
  190.    ColumnVector X(n2), Y(n2);
  191.    Real* a = A.Store(); Real* b = B.Store();  // first els of A and B
  192.    Real* an = a + n2;   Real* bn = b + n2;    // last els of A and B
  193.    Real* x = X.Store(); Real* y = Y.Store();  // first els of X and Y
  194.    Real* xn = x + i;    Real* yn = y + i;     // last els of X and Y
  195.  
  196.    Real hn = 0.5 / n2;
  197.    *x++  = hn * (*a + *an);  *y++  = - hn * (*a - *an);
  198.    a++; an--; b++; bn--;
  199.    int j = -1;  i = n2/2;
  200.    while (i--)
  201.    {
  202.       Real c,s; cossin(j--,n,c,s);
  203.       Real am = *a - *an; Real ap = *a++ + *an--;
  204.       Real bm = *b - *bn; Real bp = *b++ + *bn--;
  205.       Real samcbp = s * am - c * bp; Real sbpcam = s * bp + c * am;
  206.       *x++  =  hn * ( ap + samcbp); *y++  =  - hn * ( bm + sbpcam);
  207.       *xn-- =  hn * ( ap - samcbp); *yn-- =  - hn * (-bm + sbpcam);
  208.    }
  209.    FFT(X,Y,X,Y);             // have done inverting elsewhere
  210.    U.ReDimension(n); i = n2;
  211.    x = X.Store(); y = Y.Store(); Real* u = U.Store();
  212.    while (i--) { *u++ = *x++; *u++ = - *y++; }
  213. }
  214.  
  215.